//
//  Constraints.m
//  PezMonkey
//
//  Created by Spence DiNicolantonio on 12/29/09.
//  Copyright 2009 Necrosoft. All rights reserved.
//

#import "Constraints.h"

#define kCarCollisionGroup 1

// grid box offsets
static const PMVect boxes[] = {
	{-320, -240},
	{-160, -240},
	{0, -240},
	{160, -240},
	{-320, -120},
	{-160, -120},
	{0, -120},
	{160, -120},
	{-320, 0},
	{-160, 0},
	{0, 0},
	{160, 0},
	{-320, 120},
	{-160, 120},
	{0, 120},
	{160, 120},
};


@implementation Constraints

@synthesize staticBody;

- (NSString *)demoName {
	return @"Constraints";
}

- (void)start {
	// create space
	self.space = [PMSpace spaceWithGravity:PMVectMake(0, -1)];
	[space setSubSteps:3];
	[space setIterations:10];
	
	// initialize shared static body
	self.staticBody = [PMBody staticBody];
	
	// add static grid
	[self addGrid];
	
	NSUInteger box;
	PMBody *bodyA;
	PMBody *bodyB;
	PMBody *bodyC;
	
	PMVect posA = PMVectMake(50, 60);
	PMVect posB = PMVectMake(110, 60);
	
	
	/* Pin Joint */
	// links shapes with a solid bar or pin.
	// keeps anchor points the same distance apart from when the joint was created.
	box = 0;
	[self addBoxLabel:@"Pin Joint" box:box];
	bodyA = [self addBallAtPoint:posA box:box];
	bodyB = [self addBlockAtPoint:posB box:box];
	[space addConstraint:[PMPinJoint pinJointWithAnchor:PMVectMake(15, 0) 
												onBodyA:bodyA 
												 anchor:PMVectMake(-15, 0) 
												onBodyB:bodyB]];
	
	/* Slide Joint */
	// like pin joints but with a min/max distance.
	// can be used for a cheap approximation of a rope.
	box = 1;
	[self addBoxLabel:@"Slide Joint" box:box];
	bodyA = [self addBallAtPoint:posA box:box];
	bodyB = [self addBlockAtPoint:posB box:box];
	[space addConstraint:[PMSlideJoint slideJointWithAnchor:PMVectMake(15, 0) 
													onBodyA:bodyA 
													 anchor:PMVectMake(-15, 0) 
													onBodyB:bodyB
													  limit:PMRangeMake(20, 40)]];
	
	/* Pivot Joint */
	// holds the two anchor points together, like a swivel.
	box = 2;
	[self addBoxLabel:@"Pivot Joint" box:box];
	bodyA = [self addBallAtPoint:posA box:box];
	bodyB = [self addBlockAtPoint:posB box:box];
	PMVect pivot = PMVectAdd(boxes[box], 
							 PMVectMake(80, 60));
	[space addConstraint:[PMPivotJoint pivotJointWithBodyA:bodyA 
													 bodyB:bodyB 
													 pivot:pivot]];

	/* Groove Joint */ 
	// like a pivot joint, but one of the anchors is a line segment that the pivot 
	// can slide in
	box = 3;
	[self addBoxLabel:@"Groove Joint" box:box];
	bodyA = [self addBallAtPoint:posA box:box];
	bodyB = [self addBlockAtPoint:posB box:box];
	PMSegment groove = PMSegmentMake(PMVectMake(30, 30), 
									 PMVectMake(30, -30));
	[space addConstraint:[PMGrooveJoint grooveJointWithGroove:groove 
													  onBodyA:bodyA 
													   anchor:PMVectMake(-30, 0) 
													  onBodyB:bodyB]];
	
	/* Damped Spring */
	box = 4;
	[self addBoxLabel:@"Damped Spring" box:box];
	bodyA = [self addBallAtPoint:posA box:box];
	bodyB = [self addBlockAtPoint:posB box:box];
	[space addConstraint:[PMDampedSpring dampedSpringWithAnchor:PMVectMake(15, 0) 
														onBodyA:bodyA 
														 anchor:PMVectMake(-15, 0) 
														onBodyB:bodyB 
													 restLength:10 
													  stiffness:120
														damping:0.6]];
	/* Damped Rotary Spring */
	box = 5;
	[self addBoxLabel:@"Damped Rotary Spring" box:box];
	bodyA = [self addBarAtPoint:posA box:box];
	bodyB = [self addBarAtPoint:posB box:box];
	[space addConstraint:[PMDampedRotarySpring dampedRotarySpringWithBodyA:bodyA 
																	 bodyB:bodyB 
																 restAngle:0
																 stiffness:6000
																   damping:60]];
	
	/* Rotary Limit Joint */
	box = 6;
	[self addBoxLabel:@"Rotary Limit Joint" box:box];
	bodyA = [self addLeverAtPoint:posA box:box];
	bodyB = [self addLeverAtPoint:posB box:box];
	// hold the levers' rotations within 90 degrees of each other 
	[space addConstraint:[PMRotaryLimitJoint rotaryLimitJointWithBodyA:bodyA 
																 bodyB:bodyB 
																 limit:PMRangeMake(-M_PI_2, M_PI_2)]];
	
	/* Ratchet Joint */
	// a rotary ratchet, like a socket wrench
	box = 7;
	[self addBoxLabel:@"Ratchet Joint" box:box];
	bodyA = [self addLeverAtPoint:posA box:box];
	bodyB = [self addLeverAtPoint:posB box:box];
	// add a ratchet to the levers with teeth set every 60 degrees
	[space addConstraint:[PMRatchetJoint ratchetJointWithBodyA:bodyA
														 bodyB:bodyB
														 phase:0
													   ratchet:M_PI/3.0]];
	
	/* Gear Joint */
	// maintains a specific angular velocity ratio
	box = 8;
	[self addBoxLabel:@"Gear Joint" box:box];
	bodyA = [self addBarAtPoint:posA box:box];
	bodyB = [self addBarAtPoint:posB box:box];
	// force one bar to spin 2x the speed of the other
	[space addConstraint:[PMGearJoint gearJointWithBodyA:bodyA 
												   bodyB:bodyB 
												   phase:0 
												   ratio:2]];
	
	/* Simple Motor */
	// maintains a specific relative angular velocity
	box = 9;
	[self addBoxLabel:@"Simple Motor" box:box];
	bodyA = [self addBarAtPoint:posA box:box];
	bodyB = [self addBarAtPoint:posB box:box];
	// make the bars spin at 1/2 revolutions per second relatively
	[space addConstraint:[PMSimpleMotor simpleMotorWithBodyA:bodyA 
													   bodyB:bodyB 
														rate:M_PI]];
	
	/* Combined Constraints */
	// make a simple car with soft suspension
	box = 10;
	[self addBoxLabel:@"Combined Constraints" box:box];
	bodyA = [self addWheelAtPoint:posA box:box];
	bodyB = [self addWheelAtPoint:posB box:box];
	bodyC = [self addChassisAtPoint:PMVectMake(80, 100) box:box];
	// attach wheels to chassis with groove joints
	PMSegment grooveA = PMSegmentMake(PMVectMake(-30, -10), 
									  PMVectMake(-30, -35));
	[space addConstraint:[PMGrooveJoint grooveJointWithGroove:grooveA 
													  onBodyA:bodyC 
														bodyB:bodyA]];
	PMSegment grooveB = PMSegmentMake(PMVectMake(30, -10), 
									  PMVectMake(30, -35));
	[space addConstraint:[PMGrooveJoint grooveJointWithGroove:grooveB 
													  onBodyA:bodyC 
														bodyB:bodyB]];
	// add springs between wheels and chassis for suspension
	[space addConstraint:[PMDampedSpring dampedSpringWithAnchor:PMVectMake(-30, 0) 
														onBodyA:bodyC 
														 anchor:PMVectZero 
														onBodyB:bodyA 
													 restLength:50 
													  stiffness:150
														damping:5.5]];
	[space addConstraint:[PMDampedSpring dampedSpringWithAnchor:PMVectMake(30, 0) 
														onBodyA:bodyC 
														 anchor:PMVectZero 
														onBodyB:bodyB
													 restLength:50 
													  stiffness:150
														damping:5.5]];
	
	/* Chain */
	box = 11;
	[self addBoxLabel:@"Chain" box:box];
	bodyA = [self addBallAtPoint:posA box:box];
	bodyB = [self addBlockAtPoint:posB box:box];
	PMVect chainPoint = PMVectLerp(posA, posB, .5);
	[self addChainAtPoint:chainPoint 
					 box:box 
				 anchorA:PMVectMake(15, 0) 
				   bodyA:bodyA 
				 anchorB:PMVectMake(-15, 0) 
				   bodyB:bodyB 
				   links:8
			  linkLength:PMRangeMake(3, 8)];
	
}

/**
 * Creates a static grid of segment shapes
 */
- (void)addGrid {
	[self addStaticSegmentWithStart:PMVectMake(-320, 240) end:PMVectMake(320, 240)];
	[self addStaticSegmentWithStart:PMVectMake(-320, 120) end:PMVectMake(320, 120)];
	[self addStaticSegmentWithStart:PMVectMake(-320, 0) end:PMVectMake(320, 0)];
	[self addStaticSegmentWithStart:PMVectMake(-320, -120) end:PMVectMake(320, -120)];
	[self addStaticSegmentWithStart:PMVectMake(-320, -240) end:PMVectMake(320, -240)];
	
	[self addStaticSegmentWithStart:PMVectMake(-320, -240) end:PMVectMake(-320, 240)];
	[self addStaticSegmentWithStart:PMVectMake(-160, -240) end:PMVectMake(-160, 240)];
	[self addStaticSegmentWithStart:PMVectMake(0, -240) end:PMVectMake(0, 240)];
	[self addStaticSegmentWithStart:PMVectMake(160, -240) end:PMVectMake(160, 240)];
	[self addStaticSegmentWithStart:PMVectMake(320, -240) end:PMVectMake(320, 240)];
}

/**
 * Creates a static segment shape with a width of 0 between two given points.
 */
- (void)addStaticSegmentWithStart:(PMVect)start end:(PMVect)end {
	if (!staticBody)
		self.staticBody = [PMBody staticBody];
	
	PMShape *shape;
	
	shape = [PMSegmentShape segmentShapeWithBody:staticBody 
										   start:start 
											 end:end];
	[shape setFriction:1];
	[shape setElasticity:1];
	[shape setLayers:PMLayerMaskNotGrabbable];
	[space addShape:shape];
}

/**
 * Adds a given label to a given box.
 */
- (void)addBoxLabel:(NSString *)label box:(NSUInteger)box {
	PMVect offset = boxes[box];
	CGPoint point = NSMakePoint(offset.x + 75 - 4.5*[label length]/2,
								offset.y + 105);
	NSValue *value = [NSValue valueWithPoint:point];
	[strings setObject:label forKey:value];
}

/**
 * Creates a ball at a given point in a given box.
 */
- (PMBody *)addBallAtPoint:(PMVect)point box:(NSUInteger)box {
	// define ball attributes
	PMFloat radius = 15;
	PMFloat mass = 3;
	PMFloat moment = PMMomentForCircle(mass, 0, radius, PMVectZero);
	
	// create ball body
	PMBody *body = [PMBody bodyWithMass:mass moment:moment];
	[body setPosition:PMVectAdd(point, boxes[box])];
	[space addBody:body];
	
	// create ball shape
	PMShape *shape = [PMCircleShape circleShapeWithBody:body radius:radius];
	[shape setFriction:0.7];
	[shape setElasticity:0.0];
	[space addShape:shape];
	
	return body;
}

/**
 * Creates a block at a given point in a given box.
 */
- (PMBody *)addBlockAtPoint:(PMVect)point box:(NSUInteger)box {
	// define block attributes
	PMFloat radius = 15;
	PMFloat mass = 3;
	
	// define block vertices
	NSUInteger vertCount = 4;
	PMVect verts[] = {
		PMVectMake(-radius, -radius),
		PMVectMake(-radius, radius),
		PMVectMake(radius, radius),
		PMVectMake(radius, -radius),
	};
	
	// calculate block moment of inertia
	PMFloat moment = PMMomentForPolygon(mass, verts, vertCount, PMVectZero);
	
	// create block body
	PMBody *body = [PMBody bodyWithMass:mass moment:moment];
	[body setPosition:PMVectAdd(point, boxes[box])];
	[space addBody:body];
	
	// create block shape
	PMShape *shape = [PMPolygonShape polygonShapeWithBody:body 
												 vertices:verts 
												vertCount:vertCount];
	[shape setFriction:0.7];
	[shape setElasticity:0.0];
	[space addShape:shape];
	
	return body;
}

/**
 * Creates a lever at a given point in a given box.
 */
- (PMBody *)addLeverAtPoint:(PMVect)point box:(NSUInteger)box {
	// define lever attributes
	PMFloat mass = 3;
	PMFloat width = 10;
	PMSegment segment = PMSegmentMake(PMVectMake(0, 15),
									  PMVectMake(0, -15));
	PMFloat moment = PMMomentForSegment2(mass, segment);
	
	// create lever body
	PMBody *body = [PMBody bodyWithMass:mass moment:moment];
	[body setPosition:PMVectAdd(point, PMVectAdd(boxes[box], segment.end))];
	[space addBody:body];
	
	// create lever shape
	PMShape *shape = [PMSegmentShape segmentShapeWithBody:body 
												  segment:segment
													width:width];
	[shape setFriction:0.7];
	[shape setElasticity:0.0];
	[space addShape:shape];
	
	// add pivot joint to hold the lever in place
	PMVect pivot = PMVectAdd(boxes[box], point);
	[space addConstraint:[PMPivotJoint pivotJointWithBodyA:body 
													 bodyB:staticBody 
													 pivot:pivot]];
	
	return body;
}

/**
 * Creates a bar at a given point in a given box.
 */
- (PMBody *)addBarAtPoint:(PMVect)point box:(NSUInteger)box {
	// define bar attributes
	PMFloat mass = 3;
	PMFloat width = 10;
	PMSegment segment = PMSegmentMake(PMVectMake(0, 30),
									  PMVectMake(0, -30));
	PMFloat moment = PMMomentForSegment2(mass, segment);
	
	// create bar body
	PMBody *body = [PMBody bodyWithMass:mass moment:moment];
	[body setPosition:PMVectAdd(point, boxes[box])];
	[space addBody:body];
	
	// create bar shape
	PMShape *shape = [PMSegmentShape segmentShapeWithBody:body 
												  segment:segment
													width:width];
	[shape setFriction:0.7];
	[shape setElasticity:0.0];
	[space addShape:shape];
	
	// add pivot joint to hold the bar in place
	PMVect pivot = PMVectAdd(boxes[box], point);
	[space addConstraint:[PMPivotJoint pivotJointWithBodyA:body 
													 bodyB:staticBody 
													 pivot:pivot]];
	
	return body;
}

/**
 * Creates a car wheel at a given point in a given box.
 */
- (PMBody *)addWheelAtPoint:(PMVect)point box:(NSUInteger)box {
	// define wheel attributes
	PMFloat radius = 15;
	PMFloat mass = 1;
	PMFloat moment = PMMomentForCircle(mass, 0, radius, PMVectZero);
	
	// create wheel body
	PMBody *body = [PMBody bodyWithMass:mass moment:moment];
	[body setPosition:PMVectAdd(point, boxes[box])];
	[space addBody:body];
	
	// create wheel shape
	PMShape *shape = [PMCircleShape circleShapeWithBody:body radius:radius];
	[shape setFriction:0.7];
	[shape setElasticity:0.0];
	[shape setCollisionGroup:kCarCollisionGroup];
	[space addShape:shape];
	
	return body;
}

/**
 * Creates a car chassis at a given point in a given box.
 */
- (PMBody *)addChassisAtPoint:(PMVect)point box:(NSUInteger)box {
	// define chassis vertices
	NSUInteger vertCount = 4;
	PMVect verts[] = {
		PMVectMake(-40,-15),
		PMVectMake(-40, 15),
		PMVectMake( 40, 15),
		PMVectMake( 40,-15),
	};
	
	// define chassis attributes
	PMFloat mass = 5;
	PMFloat moment = PMMomentForPolygon(mass, verts, vertCount, PMVectZero);
	
	// create chassis body
	PMBody *body = [PMBody bodyWithMass:mass moment:moment];
	[body setPosition:PMVectAdd(point, boxes[box])];
	[space addBody:body];
	
	// create chassis shape
	PMShape *shape = [PMPolygonShape polygonShapeWithBody:body 
												 vertices:verts 
												vertCount:vertCount];
	[shape setFriction:0.7];
	[shape setElasticity:0.0];
	[shape setCollisionGroup:kCarCollisionGroup];
	[space addShape:shape];
	
	return body;
}

/**
 * Creates a chain at a given point in a given box to connect two given bodies
 * at given anchors respectively. A given number of chain links will be used that
 * have a length of a given range. 
 */
- (void)addChainAtPoint:(PMVect)point box:(NSUInteger)box 
			   anchorA:(PMAnchor)anchorA bodyA:(PMBody *)bodyA 
			   anchorB:(PMAnchor)anchorB bodyB:(PMBody *)bodyB
				 links:(NSUInteger)links
			linkLength:(PMRange)length {
	
	// calculate actual point
	point = PMVectAdd(point, boxes[box]);
	
	// define link body attributes
	PMFloat mass = 0.5;
	PMFloat radius = 3;
	PMFloat moment = PMInfinity;
	
	// calculate first link position
	PMVect pos = PMVectLerp(PMVectAdd(bodyA.position, anchorA), 
							PMVectAdd(bodyB.position, anchorB), 
							1.0 / (PMFloat)links);
	
	// create first link and attach to bodyA
	PMBody *firstLink = [PMBody bodyWithMass:mass moment:moment];
	[firstLink setPosition:pos];
	[space addBody:firstLink];
	[space addConstraint:[PMSlideJoint slideJointWithAnchor:anchorA 
													onBodyA:bodyA 
													 anchor:PMVectZero 
													onBodyB:firstLink 
													  limit:length]];
	[space addShape:[PMCircleShape circleShapeWithBody:firstLink 
												radius:radius]];
	
	// calculate last link position
	pos = PMVectLerp(PMVectAdd(bodyA.position, anchorA), 
					 PMVectAdd(bodyB.position, anchorB), 
					 (PMFloat)(links-1) / (PMFloat)links); 
	
	// create last link and attach to bodyB
	PMBody *lastLink = [PMBody bodyWithMass:mass moment:moment];
	[lastLink setPosition:pos];
	[space addBody:lastLink];
	[space addConstraint:[PMSlideJoint slideJointWithAnchor:anchorB
													onBodyA:bodyB
													 anchor:PMVectZero 
													onBodyB:lastLink 
													  limit:length]];
	[space addShape:[PMCircleShape circleShapeWithBody:lastLink 
												radius:radius]];
	
	// create middle rope links
	PMBody *previousLink = firstLink;
	PMBody *newLink;
	for (int i=2; i<links-1; i++) {
		// calculate link position
		pos = PMVectLerp(PMVectAdd(bodyA.position, anchorA), 
						 PMVectAdd(bodyB.position, anchorB), 
						 (PMFloat)i / (PMFloat)links); 
		
		// create new link
		newLink = [PMBody bodyWithMass:mass moment:moment];
		[newLink setPosition:pos];
		[space addBody:newLink];
		
		// attach new link to previous link
		[space addConstraint:[PMSlideJoint slideJointWithBodyA:previousLink 
														 bodyB:newLink 
														  limit:length]];
		[space addShape:[PMCircleShape circleShapeWithBody:newLink 
													radius:radius]];
		
		// set new previous link
		previousLink = newLink;
	}
	
	// attach previous link to last link
	[space addConstraint:[PMSlideJoint slideJointWithBodyA:previousLink 
													 bodyB:lastLink 
													 limit:length]];
}


#pragma mark -
#pragma mark Dealloc

- (void)dealloc {
	[staticBody release];
	[super dealloc];
}

@end
